#pragma once

#include <Eigen/Geometry>
#include <vector>

#include "ins_mechanization.hpp"

class InitialAlign {
private:
    std::vector<iNav::IMUData> m_static_imu_vec;
    std::vector<iNav::GnssData> m_static_gnss_vec;
    Eigen::Vector3d m_static_pos, m_static_acc;
    iNav::NavData m_initial_state;
    INS::INSMechanization* m_mechanization;

    void GetAverageStaticData();
    Eigen::Vector2d GetDeltaPositionVec(iNav::NavData initial_state, iNav::NavData final_state);
    Eigen::Vector2d GetDeltaPositionVec(iNav::NavData initial_state, iNav::GnssData final_state);

public:
    InitialAlign(std::vector<iNav::IMUData> static_imu_vec,
                 std::vector<iNav::GnssData> static_gnss_vec);
    ~InitialAlign();

    double CalculateYaw(std::vector<iNav::IMUData> dynamic_imu_vec, iNav::GnssData final_pos);
};
